//
// Created by sun on 2020/11/15.
//

#ifndef SRC_SWARMCORE_H
#define SRC_SWARMCORE_H

#include "../../include/SwarmStates.h"
#include "../OffboardManager/OffboardManager.h"
#include "ros/ros.h"
#include "px4_swarm_control/remote_uwb_info.h"
#include "px4_swarm_control/Swarm_Position.h"
#include "../../include/Utils/Serial_Utils.h"
#include "../../include/Utils/RosMath.h"
#include "../../include/Utils/RosUtils.h"
#include <px4_swarm_control/data_buffer.h>
#include <px4_swarm_control/incoming_broadcast_data.h>
#include <px4_swarm_control/sp_target_position.h>
#include "tf/transform_broadcaster.h"

using namespace px4_swarm_control;
using namespace std;
#define swarm_max_num 5

DATA_FRAM POSITION_FRAME
{
    uint8_t id;
    uint8_t takeoff_finished;
    int global_position_x;
    int global_position_y;
    int global_position_z;
    int vx;
    int vy;
    int vz;
};

enum test_state{
    first_takeoff_test,
    sencond_rotate_yaw,
    third_go_x,
    fourth_go_y,
    fifth_land
};

struct test_run_state{
    test_state cur_run;
    test_state next_run;
    bool current_run_finished;
};

class SwarmCore
{
public:
    POSITION_FRAME send_frame{};
    float global_position[3] = {0, 0, 0};
    float sp_pos[3] = {0.0, 0.0, 0.0};
    float sp_pos_v[3] = {0.0, 0.0, 0.0};
    float init_global_position[3] = {0, 0, 0};
    float init_global_yaw = 0.0;
    float v_scale = 0.5;
    float safe_r = 0.5;
    float camera_global_pos[3] = {0.0, 0.0, 0.0};
    float camera_global_yaw = 0.0;
    vector<uint8_t> swarmReadyStatus;
    ros::NodeHandle nh;
    ros::Subscriber uwb_data_sub;
    ros::Subscriber sp_pos_sub;
    ros::Publisher position_pub;
    ros::Publisher swarm_position_pub;
    ros::Subscriber world_position_sub;
    ros::Subscriber swarm_state_sub;
    incoming_broadcast_data uwb_recv_data;
    OffboardManager *om;
    SwarmStates swarm_state;
    Swarm_Position swarmPosition;
    ros::Timer swarmPositionPub_timer, ConstructSendFrameAndPub_timer;
    test_run_state run;

    SwarmCore();

    ~SwarmCore();

    void takeoff();

    void mainLoop();

    void updateParams();

    void setSpPose(const sp_target_position &msg);

    void ConstructSendFrameAndPub(const ros::TimerEvent &e);

    void parseTopicData(const incoming_broadcast_data &msg);

    void worldPositionUpdate(const geometry_msgs::PoseStamped &msg);

    void swarmStateUpdate(const remote_uwb_info &msg);
    
    void updateValues();

    void swarmPositionPub(const ros::TimerEvent &e);

    bool swarmReady();

    void initSubAndPub();

    void avoidCrash();

    void testLoop();

    vector<float> getSwarmDistance();
};

#endif //SRC_SWARMCORE_H
